#include "PhysicsManager.h"
#include "ThreadHeader.h"

//Threads for running BallBall Jobs
extern DWORD WINAPI ThreadBallBallWork( LPVOID lpParam );

//Threads for running BallWall Jobs
extern DWORD WINAPI ThreadBallWallWork( LPVOID lpParam );

//PhysicsManager::PhysicsManager(void)
//{
//	BOX_SIZE = 12.0f;
//	GRAVITY = 8.0f;
//	NUM_JOBS = 2;
//	_octree = new Octree(	Vec3f(-BOX_SIZE/2, -BOX_SIZE/2, -BOX_SIZE/2),
//						Vec3f(BOX_SIZE/2, BOX_SIZE/2, BOX_SIZE/2), 1);
//
//}

PhysicsManager::PhysicsManager(float in_Size, float in_Gravity, int in_numThreads, int in_incsize):
	GRAVITY(in_Gravity),
	BOX_SIZE(in_Size),
	NUM_JOBS(in_numThreads),
	INCSIZE(in_incsize)
{
	if(in_numThreads <=0 ){
		GetSystemInfo(&sysinfo);
		NUM_JOBS = sysinfo.dwNumberOfProcessors; //Number of processors is important for physics engine
	}

	dwThreadIdArray		= new DWORD[NUM_JOBS*2];
	hThreadArray		= new HANDLE[NUM_JOBS*2];
	hStartEvents		= new HANDLE[NUM_JOBS*2];
	hDoneEvents			= new HANDLE[NUM_JOBS*2];
	WorkerId			= new int[NUM_JOBS*2];
	hBallMoveThread		= new HANDLE[NUM_JOBS*2];
	dwBallMoveThreadId	= new DWORD[NUM_JOBS*2];
	hStartBallMove		= new HANDLE[NUM_JOBS*2];
	hStopBallMove		= new HANDLE[NUM_JOBS*2];

	BallBallWorkArray	= new vector<BallPair>[NUM_JOBS];
	BallWallWorkArray	= new vector<BallWallPair>[NUM_JOBS];
	BallsToMove			= new vector<Ball*>[NUM_JOBS*2];

	//Create worker threads here.
	createThreadJobs();

	_octree = new Octree(	Vec3f(-BOX_SIZE/2, -BOX_SIZE/2, -BOX_SIZE/2),
						Vec3f(BOX_SIZE/2, BOX_SIZE/2, BOX_SIZE/2), 1);
	//increaseBalls();
}

PhysicsManager::~PhysicsManager(void)
{
	cleanup();
   
	for(int i(0); i<NUM_JOBS; i++){
		CloseHandle(hThreadArray[i]);
		CloseHandle(hStartEvents[i]);
		CloseHandle(hDoneEvents[i]);
	}

	delete [] BallBallWorkArray;
	delete [] BallWallWorkArray;

	delete [] dwThreadIdArray;
	delete [] hThreadArray;

	delete [] hStartEvents;
	delete [] hDoneEvents;
}


int PhysicsManager::createThreadJobs(){
	int status(0);
	
	//Create event object here.  Important to create first!
	for(int i(0); i<NUM_JOBS*2; i++){
		WorkerId[i] = i;
		hStartEvents[i] = CreateEvent(
			NULL, //Security. none.
			TRUE, //Manual reset event
			FALSE,// initial state is nonsignaled
			NULL); // Object name
		hDoneEvents[i] = CreateEvent(
			NULL, //Security. none.
			TRUE, //Manual reset event
			FALSE,// initial state is nonsignaled
			NULL); // Object name
		hStartBallMove[i] = CreateEvent(
			NULL, //Security. none.
			TRUE, //Manual reset event
			FALSE,// initial state is nonsignaled
			NULL); // Object name
		hStopBallMove[i] = CreateEvent(
			NULL, //Security. none.
			TRUE, //Manual reset event
			FALSE,// initial state is nonsignaled
			NULL); // Object name

		hBallMoveThread[i] = CreateThread(
			NULL, NULL,
			&ThreadBallMove,
			&WorkerId[i],
			NULL,
			&dwBallMoveThreadId[i]);

	}

	for(int i(0); i<NUM_JOBS; i++){
		hThreadArray[i] = CreateThread(
			NULL,
			0,
			&ThreadBallBallWork,
			&WorkerId[i],
			//NULL,
			0,
			&dwThreadIdArray[i]);

		hThreadArray[i+NUM_JOBS] = CreateThread(
			NULL,
			0,
			&ThreadBallWallWork,
			&WorkerId[i+NUM_JOBS],
			//NULL,
			0,
			&dwThreadIdArray[i+NUM_JOBS]);
	}
	//TO-DO. Handle error conditions for event
	return status;
}

//Puts potential ball-ball collisions in potentialCollisions.  It must return
//all actual collisions, but it need not return only actual collisions.
void PhysicsManager::potentialBallBallCollisions(vector<BallPair> &potentialCollisions,
								 vector<Ball*> &balls, Octree* octree) {
	//Fast method
	octree->potentialBallBallCollisions(potentialCollisions);
	
	/*
	//Slow method
	for(unsigned int i = 0; i < balls.size(); i++) {
		for(unsigned int j = i + 1; j < balls.size(); j++) {
			BallPair bp;
			bp.ball1 = balls[i];
			bp.ball2 = balls[j];
			potentialCollisions.push_back(bp);
		}
	}
	*/
}

//Puts potential ball-wall collisions in potentialCollisions.  It must return
//all actual collisions, but it need not return only actual collisions.
void PhysicsManager::potentialBallWallCollisions(vector<BallWallPair> &potentialCollisions,
								 vector<Ball*> &balls, Octree* octree) {
	//Fast method
	octree->potentialBallWallCollisions(potentialCollisions);
	
	/*
	//Slow method
	Wall walls[] =
		{WALL_LEFT, WALL_RIGHT, WALL_FAR, WALL_NEAR, WALL_TOP, WALL_BOTTOM};
	for(unsigned int i = 0; i < balls.size(); i++) {
		for(int j = 0; j < 6; j++) {
			BallWallPair bwp;
			bwp.ball = balls[i];
			bwp.wall = walls[j];
			potentialCollisions.push_back(bwp);
		}
	}
	*/
}


void PhysicsManager::moveBalls(vector<Ball*> &balls, Octree* octree, float dt) {
	
	if(balls.size() > 0){
		for(unsigned int i = 0; i < balls.size(); i++) {

		BallsToMove[i%(NUM_JOBS*2)].push_back(balls[i]);
		//BallsToMove[0].push_back(balls[i]);
		//Ball* ball = balls[i];
		//Vec3f oldPos = ball->pos;
		//ball->pos += ball->v * (1.0f/dt);
		//octree->ballMoved(ball, oldPos);
		}

		//Start Threads working here.
		for(int i(0); i<NUM_JOBS*2; i++){
			SetEvent(hStartBallMove[i]);
		}

		WaitForMultipleObjects(NUM_JOBS*2, hStopBallMove, TRUE, INFINITE);

		for(int i(0); i<NUM_JOBS*2; i++){
			ResetEvent(hStopBallMove[i]);
			//ResetEvent(hDoneEvents[i+NUM_JOBS]);
		}
	}

}

//Decreases the y coordinate of the velocity of each ball by GRAVITY *
//TIME_BETWEEN_UPDATES
void PhysicsManager::applyGravity(vector<Ball*> &balls) {
	//GRAVITY OMITTED AT THIS TIME

	//for(unsigned int i = 0; i < balls.size(); i++) {
	//	Ball* ball = balls[i];
	//	ball->v -= Vec3f(0, GRAVITY * TIME_BETWEEN_UPDATES, 0);
	//}
}

bool PhysicsManager::testBallBallCollision(Ball* b1, Ball* b2){
	//Check whether the balls are close enough
	float r = b1->r + b2->r;
	if ((b1->pos - b2->pos).magnitudeSquared() < r * r) {
		//Check whether the balls are moving toward each other
		Vec3f netVelocity = b1->v - b2->v;
		Vec3f displacement = b1->pos - b2->pos;
		return netVelocity.dot(displacement) < 0;
	}
	else
		return false;
}

void PhysicsManager::handleBallBallCollisions(vector<Ball*> &balls, Octree* octree) {
	vector<BallPair> bps;
	potentialBallBallCollisions(bps, balls, octree); //Divide this Into Jobs.

	//Instead of this for loop to handle single threaded, this should combine
	//the ballpairs into jobs of bps.size()/CPUs. A flag for each CPU thread can notify
	//each thread of the 
	int num_collide = bps.size();
	//for(unsigned int i = 0; i < num_collide; i++) {
	//	BallPair bp = bps[i];
	//	
	//	Ball* b1 = bp.ball1;
	//	Ball* b2 = bp.ball2;
	//	if (testBallBallCollision(b1, b2)) {
	//		//Make the balls reflect off of each other
	//		Vec3f displacement = (b1->pos - b2->pos).normalize();
	//		b1->v -= 2 * displacement * b1->v.dot(displacement);
	//		b2->v -= 2 * displacement * b2->v.dot(displacement);
	//	}
	//}

	//distribute jobs to the holding cells
	for(unsigned int i(0); i< num_collide; i++){
		BallBallWorkArray[i%NUM_JOBS].push_back(bps[i]);
	}





}

Vec3f PhysicsManager::wallDirection(Wall wall) {
	switch (wall) {
		case WALL_LEFT:
			return Vec3f(-1, 0, 0);
		case WALL_RIGHT:
			return Vec3f(1, 0, 0);
		case WALL_FAR:
			return Vec3f(0, 0, -1);
		case WALL_NEAR:
			return Vec3f(0, 0, 1);
		case WALL_TOP:
			return Vec3f(0, 1, 0);
		case WALL_BOTTOM:
			return Vec3f(0, -1, 0);
		default:
			return Vec3f(0, 0, 0);
	}
}

//Returns whether a ball and a wall are colliding
bool PhysicsManager::testBallWallCollision(Ball* ball, Wall wall) {
	Vec3f dir = wallDirection(wall);
	//Check whether the ball is far enough in the "dir" direction, and whether
	//it is moving toward the wall
	return ball->pos.dot(dir) + ball->r > BOX_SIZE / 2 &&
			ball->v.dot(dir) > 0;
}

//Handles all ball-wall collisions
void PhysicsManager::handleBallWallCollisions(vector<Ball*> &balls, Octree* octree) {
	vector<BallWallPair> bwps;
	potentialBallWallCollisions(bwps, balls, octree);

	int num_collide = bwps.size();

	//for(unsigned int i = 0; i < bwps.size(); i++) {
	//	BallWallPair bwp = bwps[i];
	//	
	//	Ball* b = bwp.ball;
	//	Wall w = bwp.wall;
	//	if (testBallWallCollision(b, w)) {
	//		//Make the ball reflect off of the wall
	//		Vec3f dir = (wallDirection(w)).normalize();
	//		b->v -= 2 * dir * b->v.dot(dir);
	//	}
	//}

	for(unsigned int i(0); i< num_collide; i++){
		BallWallWorkArray[i%NUM_JOBS].push_back(bwps[i]);
	}

}

//Applies gravity and handles all collisions.  Should be called every
//TIME_BETWEEN_UPDATES seconds.
void PhysicsManager::performUpdate(vector<Ball*> &balls, Octree* octree) {
	//applyGravity(balls);
	//bool Waiting(true);
	//for(int i(0); i<NUM_JOBS; i++){
	//	BallBallWorkArray[i].clear();
	//	BallWallWorkArray[i].clear();
	//}

	handleBallBallCollisions(balls, octree);
	handleBallWallCollisions(balls, octree);
		//Threaded communication.

	for(int i(0); i<NUM_JOBS*2; i++){
		SetEvent(hStartEvents[i]);
		//SetEvent(hStartEvents[i+NUM_JOBS]);
	}

	WaitForMultipleObjects(NUM_JOBS*2, hDoneEvents, TRUE, INFINITE);

	for(int i(0); i<NUM_JOBS*2; i++){
		ResetEvent(hDoneEvents[i]);
		//ResetEvent(hDoneEvents[i+NUM_JOBS]);
	}

}

//Advances the state of the balls by t.  timeUntilUpdate is the amount of time
//until the next call to performUpdate.
void PhysicsManager::advance(vector<Ball*> &balls,Octree* octree, float time){
		moveBalls(balls, octree, time);
		performUpdate(balls, octree);
}

//Deletes everything.  This should be called when exiting the program.
void PhysicsManager::cleanup() {
	for(unsigned int i = 0; i < _balls.size(); i++) {
		delete _balls[i];
	}
	delete _octree;
}

void PhysicsManager::increaseBalls(){
	//Add X balls with a random position, velocity, radius, and color
	if(_balls.size() < 1000*INCSIZE){
		for(int i = 0; i < INCSIZE; i++) {
			Ball* ball = new Ball();
			ball->pos = Vec3f(2 * randomFloat() - 1,
								2 * randomFloat() - 1,
								2 * randomFloat() - 1);
			ball->v = Vec3f(4 * randomFloat() - 1,
							4 * randomFloat() - 1,
							4 * randomFloat() - 1);
			ball->r = 0.1f * randomFloat() + 0.1f;
			ball->color = Vec3f(0.6f * randomFloat() + 0.2f,
								0.6f * randomFloat() + 0.2f,
								0.6f * randomFloat() + 0.2f);
			_balls.push_back(ball);
			_octree->add(ball);
		}
	}
}

void PhysicsManager::decreaseBalls(){
	//Add X balls with a random position, velocity, radius, and color
	if(_balls.size() >= INCSIZE){
		for(int i(0); i<INCSIZE; i++){
			_octree->remove(_balls.back());
			_balls.pop_back();
		}
	}
}

float PhysicsManager::randomFloat() {
	return (float)rand() / ((float)RAND_MAX + 1);
}

int PhysicsManager::CalcFrame(float frametime){

	advance(_balls, _octree, frametime);

	return 0;
}